#include <stdio.h>
#include <unistd.h>

#include "canbus.h"
#include "osthread.h"
#include "communicate.h"
#include "robot.h"
#include "path.h"

int main(int argc, char *argv)
{
    comm_init();
    RobotSystemInfoInit();  //初始化电机参数
    global_map_create();    //初始化地图
    // insert_a_station(8);
    insert_a_station(3);
    insert_a_station(23);
    insert_a_station(28);
    thread_create();        //创建线程
   
    sleep(1);
    //while(1);
    dump_thread();          //接收数据线程（主线程）


    // insert_a_station(3);
    // insert_a_station(16);
    // show_task_station();
    // get_next_station();
    // //while(get_path_station() > 0);
    // navigation.robot_direction = P_EAST;
    // do{
    //     act_type = get_path_station();  //返回-1表示到达需要停靠的点
    //     printf("act_type: %d", (int)act_type );
    //     if(act_type == STOP)  
    //     {
    //         printf("stop\n");
    //     }
    //     else if(act_type == FORWARD){
    //         printf("FORWARD\n");
    //     }
    //     else if(act_type == BACKWARD){
    //         printf("BACKWARD\n");
    //     }
    //     else if(act_type == TURNRIGHT){
    //         printf("TURNRIGHT\n");
    //     }
    //     else if(act_type == TURNLEFT){
    //         printf("TURNLEFT\n");
    //     }
    //     else{
    //         printf("error : nvidia_handle!\n");
    //     }
    // }while(act_type > 0);


    // navigation.cur_station = 3;
    // get_next_station();

    // navigation.robot_direction = P_NORTH;
    // do{
    //     act_type = get_path_station();  //返回-1表示到达需要停靠的点
    //     printf("act_type: %d", (int)act_type );
    //     if(act_type == STOP)  
    //     {
    //         printf("stop\n");
    //     }
    //     else if(act_type == FORWARD){
    //         printf("FORWARD\n");
    //     }
    //     else if(act_type == BACKWARD){
    //         printf("BACKWARD\n");
    //     }
    //     else if(act_type == TURNRIGHT){
    //         printf("TURNRIGHT\n");
    //     }
    //     else if(act_type == TURNLEFT){
    //         printf("TURNLEFT\n");
    //     }
    //     else{
    //         printf("error : nvidia_handle!\n");
    //     }
    // }while(act_type > 0);

    return 0;
}

